﻿using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.ComponentModel;
using System.Linq;
using System.Net.Sockets;
using System.Numerics;
using System.Threading;
using System.Threading.Tasks;
using Cureos.Measures;
using Cureos.Measures.Quantities;
using InTheHand.Net.Bluetooth;
using InTheHand.Net.Sockets;
using System.Windows.Media;
namespace SensorDrone.Net
{
    /// <summary>
    /// A sensor drone implementation
    /// </summary>
    internal class SensorDrone : ISensorDrone, IDroneChannel
    {
        private const double A1 = 1.75E-3;
        private const double A2 = -1.678E-5;
        private const double Ref = 298.15;
        private const double B0 = -2.94E-5;
        private const double B1 = -5.7E-7;
        private const double B2 = 4.63E-9;
        private const double C2 = 13.4;
        private BluetoothClient _client;
        private readonly BluetoothDeviceInfo _deviceInfo;
        private readonly ConcurrentDictionary<string, ISensor> _sensors;
        private double s0 = 2.51E-14;
        private readonly CancellationTokenSource _cancellationTokenSource;

        public SensorDrone(BluetoothDeviceInfo info, CancellationTokenSource cancellationTokenSource)
        {
            _cancellationTokenSource = cancellationTokenSource;
            _sensors = new ConcurrentDictionary<string, ISensor>();
            _client = new BluetoothClient();
            _deviceInfo = info;
            Version = "?.?.?";
            Register(new BaseSensor()
            {
                Name = "Version",
                Unit = "",
                Disable = channel => true,
                Enable = channel =>  true,
                Measure = channel => ReadVersion(channel)
            });
            Register(new Sensor<ElectricPotential>(ElectricPotential.Volt, new[] { ElectricPotential.MilliVolt, ElectricPotential.Volt })
            {
               
                Name = "Battery",
                Unit = "V",
                Disable = channel =>  true,
                Enable = channel =>  true,
                Measure = channel => ReadBattery(channel)
            });

            Register(new BaseSensor
            {
                
                Name = "Charging",
                Unit = "",
                Disable = channel =>  true,
                Enable = channel =>  true,
                Measure = channel => Charging(channel)
            });
            Register(new Sensor<Humidity>(Humidity.Relative, Enumerable.Empty<Unit<Humidity>>())
            {
                
                Name = "Humidity",
                Unit = "%",
                Disable = channel => true,
                Enable = channel => true,
                Measure = channel => ReadHumidity(channel)
            });
            Register(new Sensor<Temperature>(Temperature.Celsius, new[] { Temperature.Fahrenheit, Temperature.Kelvin, Temperature.Celsius })
            {
                
                Name = "Ambient",
                Unit = "C",
                Disable = channel => true,
                Enable = channel => true,
                Measure = channel => ReadAmbient(channel)
            });
            Register(new Sensor<Pressure>(Pressure.Pascal, new[] { Pressure.KiloPascal, Pressure.Pascal,Pressure.Atmospheres,Pressure.Bar,Pressure.mmHg })
            {
                
                Name = "Pressure",
                Unit = "Pa",
                Disable = channel => DisablePressure(channel),
                Enable = channel => EnablePressure(channel),
                Measure = channel => ReadPressure(channel)
            });
            Register(new Sensor<Length>(Length.Meter, new[] { Length.Meter, Length.KiloMeter, Length.Foot, Length.Yard, Length.Inch, Length.CentiMeter })
            {

                Name = "Altitude",
                Unit = "m",
                Disable = channel => DisablePressure(channel),
                Enable = channel => EnablePressure(channel),
                Measure = channel => ReadAltitude(channel)
            });
            Register(new BaseSensor
            {

                Name = "RGBC",
                Unit = "",
                Disable = channel => DisableRgb(channel),
                Enable = channel => EnableRgb(channel),
                Measure = channel => ReadRgb(channel, "RGB")
            });

            Register(new Sensor<Illuminance>(Illuminance.Lux, new[] { Illuminance.KiloLux, Illuminance.MilliLux, Illuminance.Lux })
            {

                Name = "Lux",
                Unit = "lx",
                Disable = channel => DisableRgb(channel),
                Enable = channel => EnableRgb(channel),
                Measure = channel => ReadRgb(channel,"Lux")
            });

            Register(new Sensor<Temperature>(Temperature.Kelvin, new[] { Temperature.Celsius, Temperature.Fahrenheit, Temperature.Kelvin })
            {

                Name = "Color Temperature",
                Unit = "K",
                Disable = channel => DisableRgb(channel),
                Enable = channel => EnableRgb(channel),
                Measure = channel => ReadRgb(channel, "Color Temperature")
            });

            Register(new Sensor<Temperature>(Temperature.Kelvin, new[] { Temperature.Celsius, Temperature.Fahrenheit, Temperature.Kelvin })
            {
                
                Name = "IR",
                Unit = "K",
                Disable = channel => DisableIr(channel),
                Enable = channel  => EnableIr(channel),
                Measure = channel => ReadIr(channel)
            });
        }

        #region IDisposable Members

        public void Dispose()
        {
            _client.Close();
            _client.Dispose();
            
        }

        #endregion

        #region IDroneChannel Members

        public void Write(byte[] data)
        {
            try
            {
                NetworkStream stream = _client.GetStream();
                stream.Write(data, 0, data.Length);
            }
            catch (ObjectDisposedException)
            {
                
                
            }
            
        }

        public int Read(byte[] buffer)
        {
            NetworkStream stream = _client.GetStream();
            return stream.Read(buffer, 0, buffer.Length);
            
        }

        public byte[] Request(byte[] request)
        {
            var response = new byte[] {};
            Write(request);
            var header = new byte[2];
            Read(header);
            int length = header[1];
            var data = new byte[length];
            Read(data);
            byte command = data[0];
            if (command == 0x99)
            {
                //ProcessError
            }
            else
            {
                response = data.Skip(1).Take(data.Length - 2).ToArray();
            }
            return response;
        }

        #endregion

        #region ISensorDrone Members

        public string Address
        {
            get { return _deviceInfo.DeviceAddress.ToString(); }
        }

        public string Name
        {
            get { return _deviceInfo.DeviceName; }
        }

        public bool Connected
        {
            get { return _deviceInfo.Connected; }
        }

        async public Task<bool> ConnectAsync()
        {
            _client = new BluetoothClient();
            int retry = 3;
            bool connected = false;
            while (!connected && retry > 0)
            {
                connected = await Task<bool>.Run(() =>
                {
                    try
                    {
                        _client.Connect(_deviceInfo.DeviceAddress, BluetoothService.SerialPort);
                        return _client.Connected;
                    }
                    catch (Exception)
                    {

                        return false;
                    }


                });
                if (!connected)
                    await Task.Delay(1000);
                retry--;
            }
            if (connected)
            {
                var reading = await SampleAsync("Version");
                Version = reading.Value;
            }
            return connected;
        }

        public event Action<string, bool> Status = (s,b) => { };

        public event Action<Reading> Reading = r => { };

        public event Action<string> Error = s => { };

        public string Version { get; private set; }

        public IEnumerable<ISensor> Sensors
        {
            get { return _sensors.Select(kvp => kvp.Value); }
        }

        public bool Enable(string sensor)
        {
            if (_sensors.ContainsKey(sensor))
            {
                return _sensors[sensor].Enable(this);
            }
            throw new NotSupportedException(sensor);
        }

        public Task<bool> EnableAsync(string sensor)
        {
            return Task<bool>.Run(()=>Enable(sensor));
        }

        public async Task<IEnumerable<string>> EnableAllAsync()
        {
            var enabled = new List<string>();
            var sensors = _sensors.Values.Select(sensor => sensor.Name);
            foreach (var sensor in sensors)
            {
                if (await EnableAsync(sensor))
                {
                    enabled.Add(sensor);
                }
            }
            return enabled;
        }

        public bool Disable(string sensor)
        {
            if (_sensors.ContainsKey(sensor))
            {
                return _sensors[sensor].Disable(this);
            }
            throw new NotSupportedException(sensor);
        }

        public Reading Sample(string name)
        {
            try
            {

            
            if (_sensors.ContainsKey(name))
            {
                var sensor =  _sensors[name];
                string value = sensor.Measure(this);
                var reading = sensor.CreateReading(value);
                OnReading(reading);
                return reading;
            }
            }
            catch (Exception)
            {
                return null;
            }
            throw new NotSupportedException(name);
        }

        public Task<Reading> SampleAsync(string name)
        {
            return Task.Run(() => Sample(name));
        }
    
        public void Register(ISensor sensor)
        {
            _sensors.AddOrUpdate(sensor.Name, sensor, (name, value) => sensor);
        }

        #endregion

        public event PropertyChangedEventHandler PropertyChanged = (o,p) => { };

        private void OnReading(Reading reading)
        {
            Action<Reading> handler = Reading;
            if (handler != null) handler(reading);
        }

        public void Notify(Reading reading)
        {
            OnReading(reading);
        }

        private string ReadVersion(IDroneChannel channel)
        {
            lock (channel)
            {
                byte[] response = channel.Request(new byte[] {0x50, 0x02, 0x33, 0x00});
                if (response.Any())
                {
                    string version = string.Format("{0}.{1}.{2}", response[0], response[1], response[2]);
                    return version;
                }
                return "?.?.?";
            }
        }

        private string ReadBattery(IDroneChannel channel)
        {
            lock (channel)
            {
                byte[] response = channel.Request(new byte[] {0x50, 0x02, 0x22, 0x00});
                if (response.Any())
                {
                    int msb = response[1] & 0xff;
                    int lsb = response[0] & 0xff;
                    int value = (msb << 8) + lsb;
                    var voltage = (float) ((value/4095.0)*6.0);
                    return voltage.ToString("F2");
                }
                return "0.0";
            }
        }

        private bool DisablePressure(IDroneChannel channel)
        {
            lock (channel)
            {
                const byte i2CRegisterAddress = (byte) 0x26;
                const byte i2CWriteLength = 0x01;
                const byte disableByte = 0x00;
                byte[] call =
                {
                    0x50, 0x07, 0x11,
                    0x00, 0x60, i2CWriteLength, i2CRegisterAddress, disableByte,
                    0x00
                };
                byte[] response = channel.Request(call);
                return response.Any();
            }
        }


        private bool EnablePressure(IDroneChannel channel)
        {
            lock (channel)
            {
                const byte i2CRegisterAddress = 0x26;
                const byte i2CWriteLength = 0x01;
                const byte enableByte = 0x3f;
                byte[] enableCall =
                {
                    0x50, 0x07, 0x11,
                    0x00, 0x60, i2CWriteLength, i2CRegisterAddress, enableByte,
                    0x00
                };
                // Pick sampling rate;
                const byte selectedModeByte = 0x38; // 512ms update
                // Other options are
                // byte SELECTED_MODE_BYTE = (byte) 0x00; // 6ms update
                // byte SELECTED_MODE_BYTE = (byte) 0x08; // 10ms update
                // byte SELECTED_MODE_BYTE = (byte) 0x10; // 18ms update
                // byte SELECTED_MODE_BYTE = (byte) 0x18; // 34ms update
                // byte SELECTED_MODE_BYTE = (byte) 0x20; // 66ms update
                // byte SELECTED_MODE_BYTE = (byte) 0x28; // 130ms update
                // byte SELECTED_MODE_BYTE = (byte) 0x30; // 258ms update

                // This sets the mode
                byte[] setMode = {0x50, 0x07, 0x11, 0x00, 0x60, 0x01, 0x26, selectedModeByte, 0x00};
                // This enable data flags (allows checking pin to see if data is ready; not currently used)
                byte[] enableDataFlags = {0x50, 0x07, 0x11, 0x00, 0x60, 0x01, 0x13, 0x07, 0x00};
                // This sets the sensor from standby to active
                byte[] setActive = {0x50, 0x07, 0x11, 0x00, 0x60, 0x01, 0x26, (byte) (selectedModeByte + 0x01), 0x00};

                channel.Request(enableCall);
                channel.Request(setMode);
                channel.Request(enableDataFlags);
                byte[] response = channel.Request(setActive);
                return response.Any();
            }
        }

        private string ReadAltitude(IDroneChannel channel)
        {
            var sample = ReadPressure(channel);
            double pressure = double.Parse(sample);
            var pRatio = (double)(pressure / 101326.0);
            return ((double)((1 - Math.Pow(pRatio, 0.1902632)) * 44330.77)).ToString("F2");
        }

        private string ReadPressure(IDroneChannel channel)
        {
            lock (channel)
            {
                byte[] request = {0x50, 0x05, 0x10, 0x00, 0x60, 0x01, 0x05};
                byte[] response = channel.Request(request);
                if (response.Any())
                {
                    byte[] presByte = {response[1], response[0]};
                    // Two's compliment is easy with a BigInteger :-)
                    var bigPres = new BigInteger(presByte);
                    int presIntBits = 0x000000ff & (response[2] & 0x0c);
                    int presDecBits = 0x000000ff & (response[2] & 0x03);
                    var pascals = (float) (((int) bigPres << 2) + presIntBits + (presDecBits/4.0));
                    return pascals.ToString("F2");
                }
                return "0.0";
            }
        }

        private string ReadHumidity(IDroneChannel channel)
        {
            lock (channel)
            {
                byte[] humidityCall =
                {
                    0x50, 0x06, 0x10, 0x00, 0x40, 0xE5, 0x02, 0x00
                };
                byte[] response = channel.Request(humidityCall);
                if (response.Any())
                {
                    int msb = 0xff & (response[0]);
                    int lsb = 0xfc & (response[1]); // fc not ff
                    int adc = lsb + (msb << 8);
                    // RH above water
                    var humidity = (float) (-6.0 + 125.0*(adc/Math.Pow(2, 16)));
                    return humidity.ToString("F2");
                }
                return "0";
            }
        }

        private string Charging(IDroneChannel channel)
        {
            lock (channel)
            {
                byte[] response = channel.Request(new byte[] {0x50, 0x06, 0x10, 0x02, 0x48, 0x01, 0x02, 0x00});
                if (response.Any())
                {
                    bool charging = (byte) (response[0] & 0x04) == 0x04;
                    return charging.ToString();
                }
                return false.ToString();
            }
        }

        private bool EnableLed(IDroneChannel channel, Color color)
        {
            lock (channel)
            {
                byte[] rgbValues = {0x50, 0x08, 0x15,
                    color.R, color.G, color.B,
                    color.R, color.G, color.B,
                    0x00};
                channel.Request(rgbValues);
            }
            return true;
        }

        private bool EnableRgb(IDroneChannel channel)
        {
            lock (channel)
            {
                byte[] transistorOn = {0x50, 0x03, 0x35, 0x01, 0x00};
                byte[] powerOn = {0x50, 0x07, 0x11, 0x00, 0x39, 0x01, 0x80, 0x01, 0x00};
                //byte[] intTime_12 = { 0x50, 0x07, 0x11, 0x00, 0x39, 0x01, (byte) 0x81, 0x00, 0x00 };
                byte[] intTime100 = {0x50, 0x07, 0x11, 0x00, 0x39, 0x01, 0x81, 0x01, 0x00};
                //byte[] intTime_400 = { 0x50, 0x07, 0x11, 0x00, 0x39, 0x01, (byte) 0x81, 0x02, 0x00 };
                byte[] initAdc = {0x50, 0x07, 0x11, 0x00, 0x39, 0x01, 0x80, 0x03, 0x00};
                byte[] response = channel.Request(transistorOn);
                response = channel.Request(powerOn);
                response = channel.Request(intTime100);
                response = channel.Request(initAdc);
                return response != null && response.Length > 0;
            }
        }

        private bool DisableRgb(IDroneChannel channel)
        {
            lock (channel)
            {
                byte[] transistorOff = {0x50, 0x03, 0x35, 0x00, 0x00};
                byte[] powerOff = {0x50, 0x07, 0x11, 0x00, 0x39, 0x01, 0x80, 0x00, 0x00};
                channel.Request(transistorOff);
                byte[] response = channel.Request(powerOff);
                return response != null && response.Length > 0;
            }
        }

        private string ReadRgb(IDroneChannel channel,string reading)
        {
            lock (channel)
            {
                byte[] readColors = {0x50, 0x06, 0x10, 0x00, 0x39, 0x90, 0x08, 0x00};
                byte[] colorBytes = channel.Request(readColors);

                if (colorBytes != null && colorBytes.Length > 0)
                {
                    // Parse the output

                    double R = Bytes2Int(colorBytes[3], colorBytes[2]);
                    double G = Bytes2Int(colorBytes[1], colorBytes[0]);
                    double B = Bytes2Int(colorBytes[5], colorBytes[4]);
                    double C = Bytes2Int(colorBytes[7], colorBytes[6]);

                    // These are calibration factors measured for the absorbance loss
                    // due to the window material that the Sensordrone ships with.
                    const double rcal = 0.2639626007;
                    const double gcal = 0.2935368922;
                    const double bcal = 0.379682891;
                    const double ccal = 0.2053011829;

                    R += R*rcal;
                    G += G*gcal;
                    B += B*bcal;
                    C += C*ccal;

                    // Fancy math goes here

                    // These are calibration coefficients for three
                    // different intensity semi-full spectrum light sources.
                    // If you wanted to calibrate for a different color space,
                    // this is where the magic happens...
                    double X = -0.14282*R + 1.54924*G + -0.95641*B;
                    double Y = -0.32466*R + 1.57837*G + -0.73191*B;
                    double Z = -0.68202*R + 0.77073*G + 0.56332*B;

                    double x = X/(X + Y + Z);
                    double y = Y/(X + Y + Z);

                    double n = (x - 0.3320)/(0.1858 - y);

                    double cct = 449.0*Math.Pow(n, 3) +
                                 3525.0*Math.Pow(n, 2) +
                                 6823.3*n +
                                 5520.33;
                    if (reading == "Lux")
                    {
                        return Y.ToString("F2");
                    }
                    if (reading == "Color Temperature")
                    {
                        return cct.ToString("F2");
                    }
                    return Color.FromRgb((byte) R, (byte) G, (byte) B).ToString();
                }
                return "0";
            }
        }

        private string ReadAmbient(IDroneChannel channel)
        {
            lock (channel)
            {
                const byte i2CBank = 0x00;
                const byte i2CSlaveAddress = (byte) 0x40;
                const byte i2CRegisterReadLength = 0x02;
                const byte i2CRegisterAddress = (byte) 0xE3;
                byte[] temperatureCall =
                {
                    0x50, 0x06, 0x10,
                    i2CBank, i2CSlaveAddress,
                    i2CRegisterAddress, i2CRegisterReadLength,
                    0x00
                };


                byte[] temperatureResponse = channel.Request(temperatureCall);

                if (temperatureResponse.Any())
                {
                    // Parse data
                    int msb = 0x000000ff & temperatureResponse[0];
                    int lsb = 0x000000fc & temperatureResponse[1]; // fc not ff
                    int adc = lsb + (msb << 8);
                    double temp = adc/Math.Pow(2, 16);
                    var temperature = (float) (-46.85 + 175.72*(adc/Math.Pow(2, 16)));
                    return temperature.ToString("F2");
                }
                return double.NaN.ToString();
            }
        }

        private bool EnableIr(IDroneChannel channel)
        {
            lock (channel)
            {
                const byte i2CBank = 0x00;
                const byte i2CSlaveAddress = (byte) 0x41;
                const byte i2CStatusRegister = 0x02;
                const byte i2CWriteLength = 0x01;
                const byte enableByte = 0x75;
                byte[] enableCall =
                {
                    0x50, 0x07, 0x11,
                    i2CBank, i2CSlaveAddress, i2CWriteLength, i2CStatusRegister, enableByte,
                    0x00
                };
                byte[] response = channel.Request(enableCall);
                return response.Any();
            }
        }

        private bool DisableIr(IDroneChannel channel)
        {
            lock (channel)
            {
                const byte i2CBank = 0x00;
                const byte i2CSlaveAddress = (byte) 0x41;
                const byte i2CStatusRegister = 0x02;
                const byte disableByte = 0x00;
                const byte i2CWriteLength = 0x01;
                byte[] disableCall =
                {
                    0x50, 0x07, 0x11,
                    i2CBank, i2CSlaveAddress, i2CWriteLength, i2CStatusRegister, disableByte,
                    0x00
                };
                byte[] response = channel.Request(disableCall);
                return response.Any();
            }
        }

        // Terms used for calculating the objects Temperature

        // Equations for calculating Target Object Temperatures
        private static double S(double die, double calibrationConstant)
        {
            double sensitivity = calibrationConstant*(1 + A1*(die - Ref) + A2*(die - Ref)*(die - Ref));
            return sensitivity;
        }

        private static double VOs(double die)
        {
            double offset = B0 + B1*(die - Ref) + B2*(die - Ref)*(die - Ref);
            return offset;
        }

        private static double Seebeck(double vObj, double vOs)
        {
            double fVObj = (vObj - vOs) + C2*(vObj - vOs)*(vObj - vOs);
            return fVObj;
        }

        private bool StatusIr(IDroneChannel channel)
        {
            lock (channel)
            {


                const byte i2CBank = 0x00;
                const byte i2CSlaveAddress = (byte) 0x41;
                const byte i2CStatusRegister = 0x02;
                const byte i2CReadLength = 0x01;
                byte[] statusCall = {0x50, 0x06, 0x10, i2CBank, i2CSlaveAddress, i2CStatusRegister, i2CReadLength, 0x00};

                byte[] statusCheck = channel.Request(statusCall);

                return (0x0000000e & statusCheck[0]) == 0x0e;
            }
        }
        
        private string ReadIr(IDroneChannel channel)
        {
            lock (channel)
            {
                const byte i2CBank = 0x00;
                const byte i2CSlaveAddress = (byte) 0x41;
                const byte i2CRegisterAddressTemp = 0x01;
                const byte i2CRegisterAddressVolt = 0x00;
                const byte i2CRegisterReadLength = 0x02;
                // Getting the die temperature and the object voltage need to be done in two separate calls.
                // Making one read of twice the length will only result in headaches, tears, and an incorrect reading.
                byte[] callTemp =
                {
                    0x50, 0x06, 0x10,
                    i2CBank, i2CSlaveAddress, i2CRegisterAddressTemp, i2CRegisterReadLength,
                    0x00
                };
                byte[] callVoltage =
                {
                    0x50, 0x06, 0x10,
                    i2CBank, i2CSlaveAddress, i2CRegisterAddressVolt, i2CRegisterReadLength,
                    0x00
                };
                byte[] dataTemp = channel.Request(callTemp);
                if (!dataTemp.Any())
                {
                    return double.NaN.ToString();
                }
                byte[] dataVolt = channel.Request(callVoltage);
                if (dataVolt.Any())
                {
                    // Data is in two's complement so we should be able to use a BigInteger
                    byte[] twosCTemp = {dataTemp[1], dataTemp[0]};
                    byte[] twosCVolt = {dataVolt[1], dataVolt[0]};
                    int die = BitConverter.ToInt16(twosCTemp, 0);
                    int vObj = BitConverter.ToInt16(twosCVolt, 0);

                    // Parse the data
                    double dTDie = (die/(32.0*4.0)) + 273.15; // Should be Kelvin.
                    // The *4 was reversed engineered by me. I probably jut didn't bit shift it correctly,
                    // but, hey, the data sheet didn't tell me to.
                    double dVObj = vObj*156.25e-9; // Should be in Volts
                    double vos = VOs(dTDie);
                    double sensitivity = S(dTDie, s0);
                    double fVobj = Seebeck(dVObj, vos);
                    double tmp = dTDie*dTDie*dTDie*dTDie
                                 + (fVobj/sensitivity);
                    double temperature = Math.Sqrt(tmp);
                    temperature = Math.Sqrt(temperature);

                    // Some of you may be asking yourselves about that s0 factor above.
                    // Here is the general run down of how to get a good one.

                    // If you KNOW the temperature of the object you are point the device at
                    const double knownTemperature = 273.15;
                    // You can calculate an (X,Y) pair
                    double calX = Math.Pow(knownTemperature, 4) - Math.Pow(dTDie, 4);
                    double calY = fVobj/(1 + A1*(dTDie - Ref) + A2*(dTDie - Ref));

                    return temperature.ToString("F2");
                }
                return double.NaN.ToString();
            }
        }

        private static int Bytes2Int(byte msb, byte lsb)
        {
            int intMsb = 0x000000ff & msb;
            int intLsb = 0x000000ff & lsb;
            return ((intMsb << 8) + intLsb);
        }


        public bool Cancelled
        {
            get
            {
                return _client == null ||( _client == null && _client.Connected == false);
            }
        }


        public Task Leds(Color color)
        {
            return Task.Run(() => EnableLed(this, color));
        }
    }
}